#include "helper_functions.h"
#include "env_config.h"
#include "stdio.h"
#include "xtime_l.h"

XTime tEnd[MAX_Times] = {};
XTime tBegin[MAX_Times] = {};
u32 tUsed[MAX_Times] = {};

typedef u32(*factor_func_t)(XUwb_factor *InstancePtr, int offset, word_type *data, int length);

/**
 * @brief  A wrapper of factor array-type parameters setting
 *
 * Specify for following 3 functions:
 * - u32 XUwb_factor_Write_station_Words(XUwb_factor *InstancePtr, int offset, word_type *data, int length);
 * - u32 XUwb_factor_Write_x_range_Words(XUwb_factor *InstancePtr, int offset, word_type *data, int length);
 * - u32 XUwb_factor_Write_y_range_Words(XUwb_factor *InstancePtr, int offset, word_type *data, int length);
 *
 * @param  InstancePtr: the Factor node instance pointer
 * @param  datas: float type, the data array need to be set to factor
 * @param  length: the length of data array
 * @param  func: the function to be use to trans parameters
 */
static void factor_set_wrapper(XUwb_factor *InstancePtr, float* datas, int length, factor_func_t func){
	int i;
	data_t buf;
	for(i = 0; i < length; i++){
		buf.f = datas[i];
		func(InstancePtr, i, &(buf.u), 1);
	}
}

/**
 * @brief  the function used to initialize all factors
 *
 * @param  factors: the array of factor instance
 * @param  len: the length of that array
 *
 * @return XST_SUCCESS for all factors initialized successfully
 */
int Factors_Init(XUwb_factor* factors, int len){
	// local var
	u8 i = 0;
	int ret = XST_SUCCESS;
	// init one-by-one
	for(i = 0; i < len; i++){
		// printf("[%s]Initializing factor%d\n\r", __func__, i);
		// init data structure
		ret = XUwb_factor_Initialize(&factors[i], i);
		if(ret == XST_SUCCESS)
			// printf("[%s] factor%d Initialize Successed\n\r", __func__, i);
			{}
		else
			break;
		// set parameters in factor_node
		XUwb_factor_DisableAutoRestart(&factors[i]);
		factor_set_wrapper(&factors[i], x_range, 2, XUwb_factor_Write_x_range_Words);
		factor_set_wrapper(&factors[i], y_range, 2, XUwb_factor_Write_y_range_Words);
		factor_set_wrapper(&factors[i], stations[i], 2, XUwb_factor_Write_station_Words);
	}
	return ret;
}


/**
 * @brief  the function to initialize Variables in FG.
 *
 * @param  variables:   the pointer of variable array
 * @param  len:         the length of variable array
 *
 * @return XST_SUCCESS for successfully
 */
int Variables_Init(XUwb_variable* variables, int len){
    // local var
	u8 i = 0;
	int ret = XST_SUCCESS;
	// init one-by-one
	for(i = 0; i < len; i++){
		// printf("[%s]Initializing variable%d\n\r", __func__, i);
		// init data structure
		ret = XUwb_variable_Initialize(&variables[i], i);
		if(ret == XST_SUCCESS)
			// printf("[%s] variable%d Initialize Successed\n\r", __func__, i);
			{}
		else
			break;
		// set parameters in factor_node
        XUwb_variable_DisableAutoRestart(&variables[i]);
	}
	return ret;
}



/**
 * @brief  The function used to initialized bram_controllers
 *
 * @param  brams:   the pointer of bram controllers.
 * @param  len:     the number of bram controllers.
 *
 * @return 
 */
int Brams_Init(XBram* brams, int len){
    // local var
	u8 i = 0;
	int Status = XST_SUCCESS;
	XBram_Config *bram_Cfg;
    // init one-by-one
    for(i = 0; i < len; i++){
        /* Initialize XBram_Tester Instance */
        bram_Cfg = XBram_LookupConfig(i);
        if (bram_Cfg == (XBram_Config *) NULL) {
            Status = XST_FAILURE;
            break;
        }

        Status = XBram_CfgInitialize(&brams[i], bram_Cfg, bram_Cfg->CtrlBaseAddress);
        if (Status != XST_SUCCESS) {
            Status = XST_FAILURE;
            break;
        }
    }
    return Status;
}

/**
 * @brief  the fuction to write arrays of data to bram
 *
 * @param  bram:    the bram pointer
 * @param  pmf:     the Pmf to write to bram
 * @param  len:     the length to write
 *
 * @return 
 */
int Bram_Write_helper(XBram* bram, struct Pmf* pmf, int len){
    // local vals
    int i;
    data_t buf;
//    printf("writing to bram\n\r");
    // write data one-by-one
    for(i = 0; i < len; i++){
        buf.f = pmf->pmf[i/CARD][i%CARD];
        XBram_WriteReg(bram->Config.MemBaseAddress, i * sizeof(float), buf.u);
//        printf("%.6f ", buf.f);
//        if(i % CARD == CARD - 1){
//        	printf("\n\r");
//        }
    }
    return XST_SUCCESS;
}

/**
 * @brief  the function to read arrays of data from bram
 *
 * @param  bram:    the bram pointer
 * @param  pmf:     the Pmf to write to bram
 * @param  len:     the length to write
 *
 * @return XST_SUCCESS for successfully read.
 */
int Bram_Read_helper(XBram* bram, struct Pmf* pmf, int len){
    // local vals
    int i;
    data_t buf;
//    printf("read from bram\n\r");
    // write data one-by-one
    for(i = 0; i < len; i++){\
        buf.u = XBram_ReadReg(bram->Config.MemBaseAddress, i * sizeof(float));
        pmf->pmf[i/CARD][i%CARD] = buf.f;
//        printf("%.6f ", pmf->pmf[i/CARD][i%CARD]);
//		if(i % CARD == CARD - 1){
//			printf("\n\r");
//		}
    }
    return XST_SUCCESS;
}


/**
 * @brief  store current time as begin
 *
 * @param  index:   the index of stored time
 */
void XTime_mask(int index){
	XTime_GetTime(&tBegin[index]);
	return;
} 

/**
 * @brief  store current time as end and calculate the time used
 *
 * @param  index_b: the index of begin time
 * @param  index_e: the index of end time
 *
 * @return 
 */
u32 XTime_trigger(int index_b, int index_e){
	XTime_GetTime(&tEnd[index_e]);
	if(tEnd[index_e] < tBegin[index_b]){
		tUsed[index_e] = -1;
	} else {
		tUsed[index_e] = ((tEnd[index_e] - tBegin[index_b]) * 1e6) / COUNTS_PER_SECOND;
	}
	return tUsed[index_e];
}  

/**
 * @brief  reset the Global Timer in ZYNQ and clean the times stored
 */
void XTime_rst(){
	for(int i = 0; i < MAX_Times; i++){
		tBegin[i] = 0;
		tEnd[i] = 0;
		tUsed[i] = 0;
	}
	XTime_SetTime((XTime) 0);
	return;
}
